using LightCAD.MathLib;
using LightCAD.MathLib;
using System;
using System.Collections;
using System.Runtime.CompilerServices;

namespace LightCAD.MathLib
{
    public class Vector3 : Vector, IIOArray
    {
        public static Vector3 XAxis => new Vector3(1, 0, 0);
        public static Vector3 YAxis => new Vector3(0, 1, 0);
        public static Vector3 ZAxis => new Vector3(0, 0, 1);
        public static Vector3 Origin => new Vector3();

        #region scope properties or methods
        //private static Vector3 _vector = new Vector3();
        //private static Quaternion _quaternion = new Quaternion();
        private Vector3Context GetContext()
        {
            return ThreadContext.GetCurrThreadContext().Vector3Ctx;
        }
        #endregion

        #region Properties

        public double X;
        public double Y;
        public double Z;

        #endregion

        #region constructor
        public Vector3() : this(0, 0, 0)
        { }
        public Vector3(Vector3 v) : this(v.X, v.Y, v.Z)
        { }
        public Vector3(double x = 0, double y = 0, double z = 0)
        {
            this.X = x;
            this.Y = y;
            this.Z = z;
        }
        public Vector3(Vector2 v2, double z = 0)
        {
            this.X = v2?.X ?? 0;
            this.Y = v2?.Y ?? 0;
            this.Z = z;
        }
        #endregion

        #region methods
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Set(double x, double y, double z = double.NaN)
        {
            if (z == double.NaN) z = this.Z; // sprite.scale.set(x,y)
            this.X = x;
            this.Y = y;
            this.Z = z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetScalar(double scalar)
        {
            this.X = scalar;
            this.Y = scalar;
            this.Z = scalar;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetX(double x)
        {
            this.X = x;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetY(double y)
        {
            this.Y = y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetZ(double z)
        {
            this.Z = z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetComponent(double index, double value)
        {
            switch (index)
            {
                case 0: this.X = value; break;
                case 1: this.Y = value; break;
                case 2: this.Z = value; break;
                default: throw new Error("index is out of range: " + index);
            }
            return this;
        }
        public double GetComponent(double index)
        {
            switch (index)
            {
                case 0: return this.X;
                case 1: return this.Y;
                case 2: return this.Z;
                default: throw new Error("index is out of range: " + index);
            }
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Clone()
        {
            return new Vector3(this.X, this.Y, this.Z);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Copy(Vector3 v)
        {
            this.X = v.X;
            this.Y = v.Y;
            this.Z = v.Z;
            return this;
        }
        public Vector3 Copy4(Vector4 v)
        {
            this.X = v.X;
            this.Y = v.Y;
            this.Z = v.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Add(Vector3 v)
        {
            this.X += v.X;
            this.Y += v.Y;
            this.Z += v.Z;
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Add(double x, double y, double z)
        {
            this.X += x;
            this.Y += y;
            this.Z += z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 AddScalar(double s)
        {
            this.X += s;
            this.Y += s;
            this.Z += s;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 AddVectors(Vector3 a, Vector3 b)
        {
            this.X = a.X + b.X;
            this.Y = a.Y + b.Y;
            this.Z = a.Z + b.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 AddScaledVector(Vector3 v, double s)
        {
            this.X += v.X * s;
            this.Y += v.Y * s;
            this.Z += v.Z * s;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Sub(Vector3 v)
        {
            this.X -= v.X;
            this.Y -= v.Y;
            this.Z -= v.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SubScalar(double s)
        {
            this.X -= s;
            this.Y -= s;
            this.Z -= s;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SubVectors(Vector3 a, Vector3 b)
        {
            this.X = a.X - b.X;
            this.Y = a.Y - b.Y;
            this.Z = a.Z - b.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Mul(Vector3 v)
        {
            this.X *= v.X;
            this.Y *= v.Y;
            this.Z *= v.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 MulScalar(double scalar)
        {
            this.X *= scalar;
            this.Y *= scalar;
            this.Z *= scalar;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 MulVectors(Vector3 a, Vector3 b)
        {
            this.X = a.X * b.X;
            this.Y = a.Y * b.Y;
            this.Z = a.Z * b.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ApplyEuler(Euler euler)
        {
            return this.ApplyQuaternion(GetContext()._quaternion.SetFromEuler(euler));
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 RotateRoundAxis(Vector3 origin, Vector3 axis, double angle)
        {
            this.Sub(origin); // remove the offset
            this.ApplyAxisAngle(axis, angle); // rotate the POSITION
            this.Add(origin); // re-add the offset
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ApplyAxisAngle(Vector3 axis, double angle)
        {
            return this.ApplyQuaternion(GetContext()._quaternion.SetFromAxisAngle(axis, angle));
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ApplyMatrix3(Matrix3 m)
        {
            double x = this.X, y = this.Y, z = this.Z;
            double[] e = m.Elements;
            this.X = e[0] * x + e[3] * y + e[6] * z;
            this.Y = e[1] * x + e[4] * y + e[7] * z;
            this.Z = e[2] * x + e[5] * y + e[8] * z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ApplyNormalMatrix(Matrix3 m)
        {
            return this.ApplyMatrix3(m).Normalize();
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ApplyMatrix4(Matrix4 m)
        {
            double x = this.X, y = this.Y, z = this.Z;
            double[] e = m.Elements;
            double w = 1 / (e[3] * x + e[7] * y + e[11] * z + e[15]);
            this.X = (e[0] * x + e[4] * y + e[8] * z + e[12]) * w;
            this.Y = (e[1] * x + e[5] * y + e[9] * z + e[13]) * w;
            this.Z = (e[2] * x + e[6] * y + e[10] * z + e[14]) * w;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ApplyQuaternion(Quaternion q)
        {
            double x = this.X, y = this.Y, z = this.Z;
            double qx = q.X, qy = q.Y, qz = q.Z, qw = q.W;
            // calculate quat * vector
            double ix = qw * x + qy * z - qz * y;
            double iy = qw * y + qz * x - qx * z;
            double iz = qw * z + qx * y - qy * x;
            double iw = -qx * x - qy * y - qz * z;
            // calculate result * inverse quat
            this.X = ix * qw + iw * -qx + iy * -qz - iz * -qy;
            this.Y = iy * qw + iw * -qy + iz * -qx - ix * -qz;
            this.Z = iz * qw + iw * -qz + ix * -qy - iy * -qx;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 TransformDirection(Matrix4 m)
        {
            // input: THREE.Matrix4 affine matrix
            // vector interpreted as a direction
            double x = this.X, y = this.Y, z = this.Z;
            double[] e = m.Elements;
            this.X = e[0] * x + e[4] * y + e[8] * z;
            this.Y = e[1] * x + e[5] * y + e[9] * z;
            this.Z = e[2] * x + e[6] * y + e[10] * z;
            return this.Normalize();
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Div(Vector3 v)
        {
            this.X /= v.X;
            this.Y /= v.Y;
            this.Z /= v.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 DivScalar(double scalar)
        {
            return this.MulScalar(1 / scalar);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Min(Vector3 v)
        {
            this.X = Math.Min(this.X, v.X);
            this.Y = Math.Min(this.Y, v.Y);
            this.Z = Math.Min(this.Z, v.Z);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Max(Vector3 v)
        {
            this.X = Math.Max(this.X, v.X);
            this.Y = Math.Max(this.Y, v.Y);
            this.Z = Math.Max(this.Z, v.Z);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Clamp(Vector3 min, Vector3 max)
        {
            // assumes min < max, componentwise
            this.X = Math.Max(min.X, Math.Min(max.X, this.X));
            this.Y = Math.Max(min.Y, Math.Min(max.Y, this.Y));
            this.Z = Math.Max(min.Z, Math.Min(max.Z, this.Z));
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ClampScalar(double minVal, double maxVal)
        {
            this.X = Math.Max(minVal, Math.Min(maxVal, this.X));
            this.Y = Math.Max(minVal, Math.Min(maxVal, this.Y));
            this.Z = Math.Max(minVal, Math.Min(maxVal, this.Z));
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ClampLength(double min, double max)
        {
            double length = this.Length();
            return this.DivScalar(length == 0 ? 1 : length).MulScalar(Math.Max(min, Math.Min(max, length)));
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Floor()
        {
            this.X = Math.Floor(this.X);
            this.Y = Math.Floor(this.Y);
            this.Z = Math.Floor(this.Z);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Ceil()
        {
            this.X = Math.Ceiling(this.X);
            this.Y = Math.Ceiling(this.Y);
            this.Z = Math.Ceiling(this.Z);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Round()
        {
            this.X = Math.Round(this.X);
            this.Y = Math.Round(this.Y);
            this.Z = Math.Round(this.Z);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 RoundToZero()
        {
            this.X = (this.X < 0) ? Math.Ceiling(this.X) : Math.Floor(this.X);
            this.Y = (this.Y < 0) ? Math.Ceiling(this.Y) : Math.Floor(this.Y);
            this.Z = (this.Z < 0) ? Math.Ceiling(this.Z) : Math.Floor(this.Z);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Negate()
        {
            this.X = -this.X;
            this.Y = -this.Y;
            this.Z = -this.Z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Dot(Vector3 v)
        {
            return this.X * v.X + this.Y * v.Y + this.Z * v.Z;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double LengthSq()
        {
            return this.X * this.X + this.Y * this.Y + this.Z * this.Z;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Length()
        {
            return Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double ManhattanLength()
        {
            return Math.Abs(this.X) + Math.Abs(this.Y) + Math.Abs(this.Z);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Normalize()
        {
            var len = this.Length();

            return this.DivScalar(len == 0 ? 1 : len);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetLength(double length)
        {
            return this.Normalize().MulScalar(length);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Lerp(Vector3 v, double alpha)
        {
            this.X += (v.X - this.X) * alpha;
            this.Y += (v.Y - this.Y) * alpha;
            this.Z += (v.Z - this.Z) * alpha;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 LerpVectors(Vector3 v1, Vector3 v2, double alpha)
        {
            this.X = v1.X + (v2.X - v1.X) * alpha;
            this.Y = v1.Y + (v2.Y - v1.Y) * alpha;
            this.Z = v1.Z + (v2.Z - v1.Z) * alpha;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Cross(Vector3 v)
        {
            return this.CrossVectors(this, v);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 CrossVectors(Vector3 a, Vector3 b)
        {
            double ax = a.X, ay = a.Y, az = a.Z;
            double bx = b.X, by = b.Y, bz = b.Z;
            this.X = ay * bz - az * by;
            this.Y = az * bx - ax * bz;
            this.Z = ax * by - ay * bx;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ProjectOnVector(Vector3 v)
        {
            double denominator = v.LengthSq();
            if (denominator == 0) return this.Set(0, 0, 0);
            double scalar = v.Dot(this) / denominator;
            return this.Copy(v).MulScalar(scalar);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ProjectOnPlane(Vector3 planeNormal)
        {
            var _vector = GetContext()._vector;
            _vector.Copy(this).ProjectOnVector(planeNormal);
            return this.Sub(_vector);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Reflect(Vector3 normal)
        {
            // reflect incident vector off plane orthogonal to normal
            // normal is assumed to have unit length
            return this.Sub(GetContext()._vector.Copy(normal).MulScalar(2 * this.Dot(normal)));
        }
        [MethodImpl((MethodImplOptions)768)]
        public double AngleTo(Vector3 v)
        {
            double denominator = Math.Sqrt(this.LengthSq() * v.LengthSq());
            if (denominator == 0) return MathEx.PI / 2;
            double theta = this.Dot(v) / denominator;
            // clamp, to handle numerical problems
            return Math.Acos(MathEx.Clamp(theta, -1, 1));
        }
        [MethodImpl((MethodImplOptions)768)]
        public double DistanceTo(Vector3 v)
        {
            return Math.Sqrt(this.DistanceToSquared(v));
        }
        [MethodImpl((MethodImplOptions)768)]
        public double DistanceToSquared(Vector3 v)
        {
            double dx = this.X - v.X, dy = this.Y - v.Y, dz = this.Z - v.Z;
            return dx * dx + dy * dy + dz * dz;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double ManhattanDistanceTo(Vector3 v)
        {
            return Math.Abs(this.X - v.X) + Math.Abs(this.Y - v.Y) + Math.Abs(this.Z - v.Z);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromSpherical(Spherical s)
        {
            return this.SetFromSphericalCoords(s.Radius, s.Phi, s.Theta);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromSphericalCoords(double radius, double phi, double theta)
        {
            double sinPhiRadius = Math.Sin(phi) * radius;
            this.X = sinPhiRadius * Math.Sin(theta);
            this.Y = Math.Cos(phi) * radius;
            this.Z = sinPhiRadius * Math.Cos(theta);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromCylindrical(Cylindrical c)
        {
            return this.SetFromCylindricalCoords(c.Radius, c.Theta, c.Y);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromCylindricalCoords(double radius, double theta, double y)
        {
            this.X = radius * Math.Sin(theta);
            this.Y = y;
            this.Z = radius * Math.Cos(theta);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromMatrixPosition(Matrix4 m)
        {
            double[] e = m.Elements;
            this.X = e[12];
            this.Y = e[13];
            this.Z = e[14];
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromMatrixScale(Matrix4 m)
        {
            double sx = this.SetFromMatrixColumn(m, 0).Length();
            double sy = this.SetFromMatrixColumn(m, 1).Length();
            double sz = this.SetFromMatrixColumn(m, 2).Length();
            this.X = sx;
            this.Y = sy;
            this.Z = sz;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromMatrixColumn(Matrix4 m, int index)
        {
            return this.FromArray(m.Elements, index * 4);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromMatrix3Column(Matrix3 m, int index)
        {
            return this.FromArray(m.Elements, index * 3);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromEuler(Euler e)
        {
            this.X = e._x;
            this.Y = e._y;
            this.Z = e._z;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public override bool Equals(Vector v)
        {
            return Equals(v as Vector3);
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(Vector3 v)
        {
            return ((v.X == this.X) && (v.Y == this.Y) && (v.Z == this.Z));
        }
        public bool Equals(IIOArray obj) => Equals(obj as Vector3);

        [MethodImpl((MethodImplOptions)768)]
        public Vector3 FromArray(double[] array, int offset = 0)
        {
            this.X = array[offset];
            this.Y = array[offset + 1];
            this.Z = array[offset + 2];
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        IIOArray IIOArray.FromArray(double[] array, int offset)
        {
            return FromArray(array, offset);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double[] ToArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[3];
            array[offset] = this.X;
            array[offset + 1] = this.Y;
            array[offset + 2] = this.Z;
            return array;
        }
        [MethodImpl((MethodImplOptions)768)]
        public ListEx<double> ToJsArray(ListEx<double> array, int offset = 0)
        {
            if (array == null) array = new ListEx<double>(3);
            array[offset] = this.X;
            array[offset + 1] = this.Y;
            array[offset + 2] = this.Z;
            return array;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 ToVector2(string xname = null, string yname = null)
        {
            if (xname == null) xname = "x";
            if (yname == null) yname = "y";

            var xv = this.X;
            if (xname == "y") xv = this.Y;
            else if (xname == "z") xv = this.Z;

            var yv = this.Y;
            if (yname == "x") yv = this.X;
            else if (xname == "z") yv = this.Z;

            return new Vector2(xv, yv);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector3 Random()
        {
            this.X = MathEx.Random();
            this.Y = MathEx.Random();
            this.Z = MathEx.Random();
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 RandomDirection()
        {
            // Derived from https://mathworld.wolfram.com/SpherePointPicking.html
            double u = (MathEx.Random() - 0.5) * 2;
            double t = MathEx.Random() * MathEx.PI * 2;
            double f = Math.Sqrt(1 - u * 2);
            this.X = f * Math.Cos(t);
            this.Y = f * Math.Sin(t);
            this.Z = u;
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector3 SetFromColor(Color c)
        {

            this.X = c.R;
            this.Y = c.G;
            this.Z = c.B;

            return this;

        }
        #endregion

        public IEnumerator GetEnumerator()
        {
            yield return this.X;
            yield return this.Y;
            yield return this.Z;
        }

        [MethodImpl((MethodImplOptions)768)]
        public override bool Equals(object obj)
        {
            return this.Equals(obj as Vector3);
        }

        [MethodImpl((MethodImplOptions)768)]
        public override string ToString()
        {
            return $"x: {this.X}, y: {this.Y}, z: {this.Z}";
        }




        [MethodImpl((MethodImplOptions)768)]
        public static bool operator ==(Vector3 lhs, Vector3 rhs)
        {
            return Object.Equals(lhs, rhs);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static bool operator !=(Vector3 lhs, Vector3 rhs)
        {
            return !Object.Equals(lhs, rhs);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator +(Vector3 a, Vector3 b)
        {
            return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator -(Vector3 a, Vector3 b)
        {
            return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator -(Vector3 a)
        {
            return new Vector3(-a.X, -a.Y, -a.Z);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator *(Vector3 a, double d)
        {
            return new Vector3(a.X * d, a.Y * d, a.Z * d);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator *(double d, Vector3 a)
        {
            return new Vector3(a.X * d, a.Y * d, a.Z * d);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator /(Vector3 a, double d)
        {
            return new Vector3(a.X / d, a.Y / d, a.Z / d);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector3 operator *(Vector3 left, Vector3 right)
        {
            return new Vector3(
                left.X * right.X,
                left.Y * right.Y,
                left.Z * right.Z
            );
        }

    }
    public sealed class Vector3Context
    {
        public readonly Vector3 _vector = new Vector3();
        public readonly Quaternion _quaternion = new Quaternion();
    }
}